#include <Motor.h>
#include <Ping.h>

#define DEBUG

//Left Forward, Left Reverse, Right Forward, Right Reverse
Motor motor_Driver(2,3,4,5);

//Trigger, Echo, Max distance
UltrasonicSensor ultra(11,12,50);

int right_Count = 0;
int right_turn_count = 0;

void setup() {
    #ifdef DEBUG
        Serial.begin(115200);
    #endif  
    delay(10); 
}

void loop() {
    right_Count = 0;
    unsigned int temp = ultra.getDistance();
    #ifdef DEBUG
        Serial.print("Ping: ");
        Serial.println(temp );
    #endif

/*
I started this section with if else loops. The turns were very wide.
So i changed it to while but i have not tested it yet.
I changed the delay at the end from 5 to 500 ms so that may help also.
*/

    if(temp == 0 && right_turn_count > 4){
        turn_right_90();
        temp = 14;
        right_turn_count = 0;
    }
    while(temp > 15 && temp != 0){//I want to make a sharper right
        right_Count++;
        #ifdef DEBUG
            Serial.print("Right Count: ");
            Serial.println(right_Count);
        #endif
        if(right_Count < 5){
            #ifdef DEBUG
                Serial.println("Turning right!");
            #endif
            motor_Driver.turn_right();
            delay(30);
            motor_Driver.drive_forward();
            delay(50);
            motor_Driver.stop();
            delay(1);
            temp = ultra.getDistance();
            #ifdef DEBUG
                Serial.print("Ping: ");
                Serial.println(temp );
            #endif
        }else{
            temp = 14;            
            #ifdef DEBUG
                Serial.print("EXITING THE LOOP!!!!");
            #endif
        }        
    }
    while(temp < 10 && temp != 0){//I want to make a sharper left
        #ifdef DEBUG
            Serial.println("Turning left!");
        #endif
        motor_Driver.turn_left();
        delay(30);
        motor_Driver.drive_forward();
        delay(50);
        motor_Driver.stop();
        delay(1);
        temp = ultra.getDistance();
        #ifdef DEBUG
            Serial.print("Ping: ");
            Serial.println(temp );
        #endif 
    }

    #ifdef DEBUG
        Serial.println("Driving forward!");/*base case is driving forward*/
    #endif
    right_turn_count++;
    motor_Driver.drive_forward();
        delay(50);
}

void turn_right_90(){
    #ifdef DEBUG
      Serial.println("Making a 90 degree right!");
  #endif
   motor_Driver.drive_forward();
   delay(100);
   motor_Driver.turn_right();
   delay(500);
   motor_Driver.drive_forward();
   delay(75);
}
